/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-01-15     Wangjiangbo       the first version
 */
#include <app/app_eleact.h>

#include <dayond/dayond.h>

enum RUN_STATE {
    TO_MC_DISABLE = 0,
    MC_DISABLE,
    TO_MC_ENABLE,
    MC_ENABLE,
};

enum RUN_CMD {
    CMD_NONE = 0,
    CMD_ENABLE = 1,
    CMD_DISABLE,
    CMD_SET_VEL,
};


static rt_timer_t timer;
static rt_sem_t sem;
static int32_t cmd;
static int32_t cmd_dir;

static void timeout (void *parameter)
{
    rt_sem_release(sem);
}

/* 线程入口 */
static void thread_entry(void* parameter)
{
    int32_t motor_vel_limit = 0;
    int32_t motor_dir = 0;
    int32_t motor_cmd = 0;
    int8_t state = TO_MC_DISABLE;

    rt_thread_mdelay(2000);

    motor_vel_limit = 100;

    dy_init();

    if (timer != RT_NULL)
        rt_timer_start(timer);

    while (1)
    {
        if(rt_sem_take(sem, RT_WAITING_FOREVER) != RT_EOK)
            continue;

        rt_enter_critical();

        motor_dir = cmd_dir;
        motor_cmd = cmd;
        cmd = CMD_NONE;
        cmd_dir = 0;
        rt_exit_critical();


        switch(state)
        {
        case TO_MC_DISABLE:
            dy_all_disable();
            state = MC_DISABLE;
            break;

        case MC_DISABLE:

            if(motor_cmd ==CMD_ENABLE)
                state = TO_MC_ENABLE;
            break;

        case TO_MC_ENABLE:

            dy_all_enable();
            dy_all_run(0);

            state = MC_ENABLE;

            break;

        case MC_ENABLE:

            if(motor_dir != 0)
            {
                dy_all_run(motor_vel_limit * motor_dir);
            }
            else
            {
                dy_all_run(0);
            }

            if(motor_cmd == MC_DISABLE)
                state = TO_MC_DISABLE;

            break;

        default:
            break;
        }
    }
}

void app_eleact_init(void)
{
#define THREAD_STACK_SIZE   512
#define THREAD_PRIORITY     8
#define THREAD_TIMESLICE    9

    rt_thread_t tid = RT_NULL;

    // 创建信号量
    sem = rt_sem_create("eact_sem", 0, RT_IPC_FLAG_FIFO);

    // 创建线程
    tid = rt_thread_create("eact control",
                            thread_entry, RT_NULL,
                            THREAD_STACK_SIZE,
                            THREAD_PRIORITY, THREAD_TIMESLICE);

    // 创建定时器  周期定时器
    timer = rt_timer_create("eact_time", timeout,
                             RT_NULL,
                             rt_tick_from_millisecond(250),
                             RT_TIMER_FLAG_PERIODIC);

    if (tid != RT_NULL)
        rt_thread_startup(tid);

}

void app_eleact_enable(void)
{
    rt_enter_critical();
    cmd = CMD_ENABLE;
    rt_exit_critical();
}

void app_eleact_disable(void)
{
    rt_enter_critical();
    cmd = CMD_DISABLE;
    rt_exit_critical();
}

void app_eleact_run(int8_t dir)
{
    rt_enter_critical();
    cmd = CMD_SET_VEL;
    cmd_dir = dir;
    rt_exit_critical();
}

